#include "../utils/QueueUtil.h"
#include <math.h>
#include <stdbool.h>
#include <cstring>
#include "default.h"
//#include "imu_smooth.h"
#include "horizon.h"
#include <iostream>
#include <string>

#define M_PI    3.141592653

#define MAX_VELOCITY 500
#define RAD_TO_DEG (180.0 / M_PI)
#define SMOOTH_BUFFER_SIZE 30
#define SAMPLE_RATE 200
#define BUFFER_SIZE 15
#define alpha_0_1s_param 0.1
#define smoothness_param 0.5
#define max_smoothness_param 1

//Queue globalQueue;

typedef struct SmoothingAlgorithm SmoothingAlgorithm;
typedef struct HorizonLock HorizonLock;

typedef struct MySmoothing {
    Quaternion* quaternions;
    Quaternion* smoothed_quaternions;
    Quaternion* org_smoothed_quaternions;
    bool use_gravity_vectors;
    // Other members as needed
} MySmoothing;

DefaultAlgo initDefaultAlgo() {
    DefaultAlgo algo;
    algo.smoothness = 0.5;
    algo.smoothness_pitch = 0.5;
    algo.smoothness_yaw = 0.5;
    algo.smoothness_roll = 0.5;
    algo.per_axis = false;
    algo.second_pass = false;
    algo.trim_range_only = false;
    algo.max_smoothness = 1.0;
    algo.alpha_0_1s = 0.1;
    return algo;
}

// 这段代码的目的是计算一个平滑处理的参数 alpha，用于在信号处理或其他领域中实现平滑效果，控制信号的变化速度或衰减速度。
// 这个参数的值通常受到时间常数和采样率的影响。
// .exp() 函数对这个值进行指数运算，即计算 e 的指数幂。因为指数值是负数，e指数曲线在负半轴的值在0-1间，所以这一步用于将一个值映射到 (0, 1) 范围内。
float get_alpha(float time_constant, float sample_rate) {
    return 1.0f - exp(-(1.0f / sample_rate) / time_constant);
}


// 函数：计算四元数的范数
double quaternion_norm(Quaternion q) {
    return sqrt(q.q0 * q.q0 + q.q1 * q.q1 + q.q2 * q.q2 + q.q3 * q.q3);
}

//// 函数：四元数插值（Slerp）
//Quaternion quaternion_slerp(Quaternion q1, Quaternion q2, double t) {
//    // 首先确保两个四元数都是单位四元数（范数为1）
//    q1.q0 /= quaternion_norm(q1);
//    q1.q1 /= quaternion_norm(q1);
//    q1.q2 /= quaternion_norm(q1);
//    q1.q3 /= quaternion_norm(q1);
//
//    q2.q0 /= quaternion_norm(q2);
//    q2.q1 /= quaternion_norm(q2);
//    q2.q2 /= quaternion_norm(q2);
//    q2.q3 /= quaternion_norm(q2);
//
//    // 计算角度差
//    double dot_product = q1.q0 * q2.q0 + q1.q1 * q2.q1 + q1.q2 * q2.q2 + q1.q3 * q2.q3;
//
//    // 如果点积小于零，则反转一个四元数以选择短弧路径
//    if (dot_product < 0) {
//        q2.q0 = -q2.q0;
//        q2.q1 = -q2.q1;
//        q2.q2 = -q2.q2;
//        q2.q3 = -q2.q3;
//        dot_product = -dot_product;
//    }
//
//    // 插值
//    double theta = acos(dot_product);
//    double sin_theta = sin(theta);
//    double t1 = sin((1 - t) * theta) / sin_theta;
//    double t2 = sin(t * theta) / sin_theta;
//
//    Quaternion result;
//    result.q0 = q1.q0 * t1 + q2.q0 * t2;
//    result.q1 = q1.q1 * t1 + q2.q1 * t2;
//    result.q2 = q1.q2 * t1 + q2.q2 * t2;
//    result.q3 = q1.q3 * t1 + q2.q3 * t2;
//
//    return result;
//}



/**
 * 计算四元数对应的旋转角度
 * @param quat
 * @return 返回角度，单位弧度rad
 */
double angle(const Quaternion *quat) {
    // 计算实数部分的绝对值
    double w = fabs(quat->q0);
    // 计算虚数部分的模长
    double norm = sqrt(quat->q1 * quat->q1 + quat->q2 * quat->q2 + quat->q3 * quat->q3);
    // 计算虚数部分的 atan2
    double angle = atan2(norm, w) * 2.0;
    return angle;
}


// 需要计算时间常量的函数，时间常量关系到  速度平滑  时的滤波器带宽 alpha
// 答：   时间常量τ  是单位阶跃函数的平滑响应达到原始信号的 1-1/e 以下 所需的时长，
//      是滤波器设计时的特性之一，古老的回忆



/**
 * 四元数转换为欧拉角，按照 XYZ 旋转顺序（roll、pitch、yaw）
 * @param q
 * @param roll
 * @param pitch
 * @param yaw
 */
void quaternion_to_euler_xyz(EulerAngle* a, Quaternion* q) {
    // Normalize the quaternion to ensure it has unit length
    double norm = sqrt(q->q1 * q->q1 + q->q2 * q->q2 + q->q3 * q->q3 + q->q0 * q->q0);
    q->q0 /= norm;
    q->q1 /= norm;
    q->q2 /= norm;
    q->q3 /= norm;

    // Calculate the roll (X-axis rotation)
    a->roll = atan2(2.0 * (q->q1 * q->q2 + q->q3 * q->q0), 1.0 - 2.0 * (q->q2 * q->q2 + q->q3 * q->q3));

    // Calculate the pitch (Y-axis rotation)
    a->pitch = asin(2.0 * (q->q1 * q->q3 - q->q2 * q->q0));

    // Calculate the yaw (Z-axis rotation)
    a->yaw = atan2(2.0 * (q->q1 * q->q0 + q->q2 * q->q3), 1.0 - 2.0 * (q->q3 * q->q3 + q->q0 * q->q0));
}



/**
 * 平滑角速度的函数
 * @param velocity
 * @param length
 * @param alpha_0_1s
 */
void smoothVelocity(Vector3* velocity, size_t length, double alpha_0_1s) {
    if (length == 0) {
        return;
    }
    // 平滑过程正向遍历
    Vector3 prev_velocity = velocity[0];
    for (size_t i = 1; i < length; i++) {
        velocity[i].x = prev_velocity.x * (1.0 - alpha_0_1s) + velocity[i].x * alpha_0_1s;
        velocity[i].y = prev_velocity.y * (1.0 - alpha_0_1s) + velocity[i].y * alpha_0_1s;
        velocity[i].z = prev_velocity.z * (1.0 - alpha_0_1s) + velocity[i].z * alpha_0_1s;
        prev_velocity = velocity[i];
    }
    // 平滑过程反向遍历
    prev_velocity = velocity[length - 1];
    for (size_t i = length - 2; i < length - 1; i--) {
        velocity[i].x = prev_velocity.x * (1.0 - alpha_0_1s) + velocity[i].x * alpha_0_1s;
        velocity[i].y = prev_velocity.y * (1.0 - alpha_0_1s) + velocity[i].y * alpha_0_1s;
        velocity[i].z = prev_velocity.z * (1.0 - alpha_0_1s) + velocity[i].z * alpha_0_1s;
        prev_velocity = velocity[i];
    }
}

Quaternion quat_inverse(Quaternion q) {
    Quaternion result;
    result.q0 = q.q0;     // w
    result.q1 = -q.q1;    // x
    result.q2 = -q.q2;    // y
    result.q3 = -q.q3;    // z
    return result;
}

// Function to perform quaternion multiplication
Quaternion quat_multiply(Quaternion q1, Quaternion q2) {
    Quaternion result;
    result.q0 = q1.q0 * q2.q0 - q1.q1 * q2.q1 - q1.q2 * q2.q2 - q1.q3 * q2.q3;   //w
    result.q1 = q1.q0 * q2.q1 + q1.q1 * q2.q0 + q1.q2 * q2.q3 - q1.q3 * q2.q2;   //x
    result.q2 = q1.q0 * q2.q2 - q1.q1 * q2.q3 + q1.q2 * q2.q0 + q1.q3 * q2.q1;   //y
    result.q3 = q1.q0 * q2.q3 + q1.q1 * q2.q2 - q1.q2 * q2.q1 + q1.q3 * q2.q0;   //z

    return result;
}

double quaternion_magnitude(Quaternion q) {
    return sqrt(q.q0 * q.q0 + q.q1 * q.q1 + q.q2 * q.q2 + q.q3 * q.q3);
}

// Slerp 插值函数
Quaternion slerp(Quaternion q1, Quaternion q2, double t) {
    // 计算插值参数的边界
    if (t <= 0.0) return q1;
    if (t >= 1.0) return q2;

    // 计算插值角度
    double cos_theta = q1.q0 * q2.q0 + q1.q1 * q2.q1 + q1.q2 * q2.q2 + q1.q3 * q2.q3;

    // 如果 cos_theta < 0，则翻转一个四元数以获得最短路径插值
    if (cos_theta < 0) {
        q1.q0 = -q1.q0;
        q1.q1 = -q1.q1;
        q1.q2 = -q1.q2;
        q1.q3 = -q1.q3;
        cos_theta = -cos_theta;
    }

    // 插值角度
    double theta = acos(cos_theta);
    double sin_theta = sin(theta);

    // 插值
    double weight_q1 = sin((1.0 - t) * theta) / sin_theta;
    double weight_q2 = sin(t * theta) / sin_theta;

    Quaternion result;
    result.q0 = q1.q0 * weight_q1 + q2.q0 * weight_q2;
    result.q1 = q1.q1 * weight_q1 + q2.q1 * weight_q2;
    result.q2 = q1.q2 * weight_q1 + q2.q2 * weight_q2;
    result.q3 = q1.q3 * weight_q1 + q2.q3 * weight_q2;

    return result;
}

// 计算四元数的模
double quaternionNorm(const Quaternion *q) {
    return sqrt(q->q0 * q->q0 + q->q1 * q->q1 + q->q2 * q->q2 + q->q3 * q->q3);
}

// 计算四元数的夹角（弧度）
double quaternionAngle(const Quaternion *q1, const Quaternion *q2) {
    // 计算两个四元数的差
    Quaternion deltaQ;
    deltaQ.q0 = q1->q0 * q2->q0 + q1->q1 * q2->q1 + q1->q2 * q2->q2 + q1->q3 * q2->q3;
    deltaQ.q1 = q1->q0 * q2->q1 - q1->q1 * q2->q0 - q1->q2 * q2->q3 + q1->q3 * q2->q2;
    deltaQ.q2 = q1->q0 * q2->q2 + q1->q1 * q2->q3 - q1->q2 * q2->q0 - q1->q3 * q2->q1;
    deltaQ.q3 = q1->q0 * q2->q3 - q1->q1 * q2->q2 + q1->q2 * q2->q1 - q1->q3 * q2->q0;

    // 计算 deltaQ 的模
    double deltaNorm = quaternionNorm(&deltaQ);

    // 计算夹角（弧度）
    return 2.0 * atan2(deltaNorm, q1->q0 * q2->q0 + q1->q1 * q2->q1 + q1->q2 * q2->q2 + q1->q3 * q2->q3);
}
/**
 * 平滑算法的总入空函数
 */
std::string alg_smooth(Queue* customerQ) {

    std::string result = "";
    //用来将double等数值转成string以拼接日志的中间变量
    char buffer[10];

    Quaternion current_q;
    Quaternion quats[BUFFER_SIZE];
    Quaternion interpolated_quats[BUFFER_SIZE];
    double angleVelocity[BUFFER_SIZE];

    //prevAv用以保存15个角速度序列的第一个值，or 上一个15序列小组的最后一个值；故初始化为0
    double prevAv = 0.0;
    int i;
    //初始角速度为0
    angleVelocity[0] = 0;
    Quaternion dist, prevQuaternion, prevQuaternionInv;

    //初始化各类算法参数
    float alpha_0_1s, alpha_smoothness;
    alpha_0_1s = get_alpha((float)alpha_0_1s_param, (float)SAMPLE_RATE);
    alpha_smoothness = get_alpha((float)max_smoothness_param, (float)SAMPLE_RATE);

    //2.
    // 初始化velocity序列 ,用以保存基于前面互补滤波后的imu数据，得出的当前时刻的角速度
    // 这里的序列长度可调
    // 且是从globalQueue中拷贝出来的局部序列
    // 建模方式是 序列分段 15或30帧为一组，算一次
    // 若算力来得及，也可以尝试 15或30个序列一组，每次只为得到第一个值的滤波后数据，即为了一个值，要维持一个序列，且序列其他值，每次计算的结果，均被丢弃

    //将长度为15的序列 赋值给本地quats[]
    int index = 0;
    double angVel;
    int timeStamp;
    while (customerQ->length > 0 && index < BUFFER_SIZE) {
        Quaternion q;
        Quaternion diffQ;
        dequeue(customerQ, &timeStamp, &q, &diffQ);
        quats[index] = q;

        index++;
    }

    prevQuaternion = quats[0];

    for(i = 0; i < BUFFER_SIZE; i++) {
        prevQuaternionInv = quat_inverse(prevQuaternion);
        dist = quat_multiply(prevQuaternionInv, quats[i]);

        // 为了防止acos(1) = NaN, 对acos的入参做保护
        angleVelocity[i] = 2 * acos(dist.q0 < 1 ? dist.q0 : 1) * RAD_TO_DEG * SAMPLE_RATE;

        double angle = quaternionAngle(&prevQuaternion, &quats[i]);

        prevQuaternion = quats[i];

        sprintf(buffer, "%f", angleVelocity[i]);
        result += buffer;
//        result += '\t';
//        sprintf(buffer, "%f", angle);
//        result += buffer;
//        result += '\t\n';

    }


    //3.
    // 将velocity进行双向平滑
    // 第一个循环，从第二个元素开始向前遍历
    double prev_velocity = angleVelocity[0];
    for (i = 0; i < BUFFER_SIZE; i++) {
        double vel = angleVelocity[i];
        angleVelocity[i] = prev_velocity * (1.0 - alpha_0_1s) + vel * alpha_0_1s;
        prev_velocity = angleVelocity[i];
    }

    // 第二个循环，从倒数第二个元素向后遍历
    prev_velocity = angleVelocity[BUFFER_SIZE - 1];
    for (int i = BUFFER_SIZE - 2; i >= 0; i--) {
        double vel = angleVelocity[i];
        angleVelocity[i] = prev_velocity * (1.0 - alpha_0_1s) + vel * alpha_0_1s;
        prev_velocity = angleVelocity[i];
    }

       //4.
       // 依次对velocity中的velocity进行归一化
       double avNormal = MAX_VELOCITY * smoothness_param;
       for (i = 0; i < BUFFER_SIZE; i++) {
           angleVelocity[i] = angleVelocity[i] / avNormal;
       }


       //5.
       // (plain 3D) 双向简单指数滤波
       //第一遍正向
       double val;
       for (i = 0; i < BUFFER_SIZE; i++) {
           val = alpha_smoothness * (1.0 - angleVelocity[i]) + alpha_0_1s * angleVelocity[i];
           current_q = slerp(current_q, quats[i], val < 1.0 ? val : 1.0);
           interpolated_quats[i] = current_q;
       }

       //第二遍逆向
       for (i = BUFFER_SIZE - 1; i >= 0; i--) {
           val = alpha_smoothness * (1.0 - angleVelocity[i]) + alpha_0_1s * angleVelocity[i];
           current_q = slerp(current_q, interpolated_quats[i], val < 1.0 ? val : 1.0);
           quats[i] = current_q;
       }

       // todo: 依次计算平滑后四元数与输入四元数之间的旋转四元数，存入sq这个队列，最终给到rolling_shutter 的是sq
       for (int i = 0; i < BUFFER_SIZE; i++) {
           //依次计算平滑后四元数与输入四元数之间的旋转四元数，存入sq这个队列，返回的是sq
           // rotation quaternion from smooth motion -> raw motion to counteract it
//           invert_and_multiply_quaternions(&self->smoothed_quaternions[i], &self->quaternions[i], &self->org_smoothed_quaternions[i]);
       }



    // ////////////////////////////////////////////////////////////
    // 截止以上，完成一阶指数平滑
    // ////////////////////////////////////////////////////////////
    // 若启用二阶指数平滑，则继续执行以下部分代码
    // ////////////////////////////////////////////////////////////


    //todo: 6. 把原始imu序列的每一个quaternion的逆，依次和双向简单指数滤波之后的smoothed2的quaternion 相乘，
//              得到二者之间的旋转值，存入一个新的distance序列。
//              且，顺便在遍历过程中，得到最大的max_distance(角度degree)

    //todo: 7. 遍历distance，每个旋转角度除以max_distance, 对于小于0.5的，直接置0

    //todo: 8. 将distance平滑，即双向简单低通滤波

    //todo: 9. 再次遍历distance，获取最大max_distance

    //todo: 10. 再次遍历distance，将每个值依次归一化，再令 dist = （dist + 1）/ 2

    //todo: 11. Plain 3D smoothing with varying alpha, 对smooth2再来一次
    //          这次和前一次滤波的区别主要在，本次除了velocity外，还增加了distance
    //

    result += '\n';
    return result;
}


/*************************************************************************************************************/
/*************************************        test cases      ************************************************/
/*************************************************************************************************************/

/**
 * 测试 平滑角速度方法
 * @return
 */
int test_smoothVelocity() {
    // 示例数据
    Vector3 velocity[] = {
            {1.0, 2.0, 3.0},
            {2.0, 3.0, 4.0},
            {3.0, 4.0, 5.0}
    };
    size_t length = sizeof(velocity) / sizeof(velocity[0]);
    double alpha_0_1s = 0.1;

    // 调用平滑函数
    smoothVelocity(velocity, length, alpha_0_1s);

    // 打印平滑后的结果
    for (size_t i = 0; i < length; ++i) {
        printf("Smoothed Velocity[%zu]: (%.2f, %.2f, %.2f)\n", i, velocity[i].x, velocity[i].y, velocity[i].z);
    }

    return 0;
}


/*

void recompute_smoothness(MySmoothing* self, SmoothingAlgorithm* alg, HorizonLock* horizon_lock, StabilizationParams* stabilization_params, KeyframeManager* keyframes) {
    if (true) {
        // Lock horizon, then smooth
        // Replace the following with your actual logic

//        integration_method: 1,
//        use_gravity_vectors: false,
//        horizon_lock_integration_method: 1, // VQF
//        integration_method是采用的imu_track方法，原工程采用vqf，即1，本项目imu_track采用互补滤波，可能是2。
//        代码中再具体看究竟是如何使用方法序号的。在获取方法的地方，直接使用互补滤波即可
//        horizon_lock.lock(&self.quaternions,
//                          &self.quaternions,
//                          &self.file_metadata.gravity_vectors,
//                          self.use_gravity_vectors,
//                          self.integration_method,
//                          keyframes,
//                          stabilization_params);

        //  操作界面默认lock_enable 是false
        //  鉴于本方法嵌套太多
        //  因此，先跳过“锁定地平线”方法的具体实现，注释掉
//        horizon_lock_lock(horizon_lock, self->quaternions, self->quaternions, self->file_metadata.gravity_vectors, self->use_gravity_vectors, self->integration_method, keyframes, stabilization_params);
        alg_smooth(alg, self->smoothed_quaternions, self->duration_ms, stabilization_params, keyframes);
    }

    // 这段代码 遍历两个集合self.smoothed_quaternions 和 self.quaternions
    // 对self.smoothed_quaternions 和 self.quaternions 这两个集合中的每对四元数之间执行一些数学运算，
    // 产生新的四元数并存储在 self.smoothed_quaternions 中
    // 1. self.smoothed_quaternions.iter_mut() 返回了 self.smoothed_quaternions 的可变迭代器，允许对集合中的元素进行可变访问。
    // 2. self.quaternions.iter() 返回了 self.quaternions 的不可变迭代器，用于读取集合中的元素。
    // 3. zip 方法将两个迭代器进行配对，生成一个新的迭代器，该迭代器将同时迭代两个集合，并将它们的元素一一对应起来。
    //    这样，for 循环就可以同时处理两个集合的元素。
    // 4. 在 for 循环的每一次迭代中，(sq, q) 是一个元组，
    //    其中 sq 是 self.smoothed_quaternions 中的一个可变引用，
    //    而 q 是 self.quaternions 中的一个不可变引用。
    // 循环内的计算过程:
    // 1. 从 self.smoothed_quaternions 中获取一个四元数，并通过 sq.1 访问这个四元数的第二个元素。
    //    四元数通常由四个分量组成，这里假设是 (w, x, y, z)，
    //    因此 sq.1 可能是一个 Quaternion 结构体中的 x、y、z 分量的引用。
    // 2. 从 self.quaternions 中获取一个四元数，并通过 q.1 访问这个四元数的第二个元素。
    // 3. 执行四元数的操作，具体是将 sq.1 修改为 sq.1 的逆乘以 q.1。
    //    这个操作可能表示通过一种数学计算来将平滑的四元数转换回原始的四元数以抵消它。

//    for (int i = 0; i < NUM_QUATERNIONS; i++) {
//        //依次计算平滑后四元数与输入四元数之间的旋转四元数，存入sq这个队列，返回的是sq
//        // rotation quaternion from smooth motion -> raw motion to counteract it
//        invert_and_multiply_quaternions(&self->smoothed_quaternions[i], &self->quaternions[i], &self->org_smoothed_quaternions[i]);
//    }

}

*/




